using System;

using Microsoft.SPOT;
using Microsoft.SPOT.Input;
using Microsoft.SPOT.Presentation;
using Microsoft.SPOT.Presentation.Controls;

namespace Robot
{
    public class Program : Microsoft.SPOT.Application
    {
//        DeviceSolutions.SPOT.Hardware.Meridian.PWM pwm = new DeviceSolutions.SPOT.Hardware.Meridian.PWM();
        RobotController controller = new RobotController(DeviceSolutions.SPOT.Hardware.Meridian.Pins.GPIO4, DeviceSolutions.SPOT.Hardware.Meridian.Pins.GPIO10);
        byte m_speed = 0;
        byte m_direction = 0;
        SerialListener m_listener = new SerialListener();
//        DistanceSensor m_distance = new DistanceSensor();
        delegate void ByteReceivedDelegate(byte value);
        ByteReceivedDelegate m_byteReceivedDelegate;
//        delegate void DistanceUpdatedDelegate(int value);
//        DistanceUpdatedDelegate m_distanceUpdatedDelegate;
//        const int SAFE_DISTANCE = 40;
        WatchDog m_watchdog_on = new WatchDog();
        WatchDog m_watchdog_off = new WatchDog();

        public static void Main()
        {
            Program myApplication = new Program();

            Window mainWindow = myApplication.CreateWindow();

            Microsoft.SPOT.Touch.Touch.Initialize(myApplication);

            // Create the object that configures the GPIO pins to buttons.
            GPIOButtonInputProvider inputProvider = new GPIOButtonInputProvider(null);

            // Start the application
            myApplication.Run(mainWindow);
        }

        private Window mainWindow;

        public Window CreateWindow()
        {
            // Create a window object and set its size to the
            // size of the display.
            mainWindow = new Window();
            mainWindow.Height = SystemMetrics.ScreenHeight;
            mainWindow.Width = SystemMetrics.ScreenWidth;

            // Connect the button handler to all of the buttons.
            mainWindow.AddHandler(Buttons.ButtonUpEvent, new ButtonEventHandler(OnButtonUp), false);

            // Set the window visibility to visible.
            mainWindow.Visibility = Visibility.Visible;

            // Attach the button focus to the window.
            Buttons.Focus(mainWindow);

            controller.Speed = m_speed;

            m_byteReceivedDelegate = new ByteReceivedDelegate(listener_ByteReceived);
//            m_distanceUpdatedDelegate = new DistanceUpdatedDelegate(m_distance_DistanceChanged);

            m_listener.ByteReceived += new SerialListener.ByteReceivedHandler(listener_ByteReceived);
            m_listener.Start();

//            m_distance.DistanceChanged += new DistanceSensor.DistanceChangedHandler(m_distance_DistanceChanged);
//            m_distance.Start();

            AddControls(ref mainWindow);

            m_watchdog_on.TimeOut += new WatchDog.TimeOutHandler(m_watchdog_on_TimeOut);

            m_watchdog_off.TimeOut += new WatchDog.TimeOutHandler(m_watchdog_off_TimeOut);

            return mainWindow;
        }

        /// <summary>
        /// Turns backlight on
        /// </summary>
        void m_watchdog_on_TimeOut()
        {
            m_watchdog_off.Stop();
            Microsoft.SPOT.Hardware.Utility.Backlight(true);
            Stop();
            m_watchdog_on.Stop();
        }

        /// <summary>
        /// Turns backlight off
        /// </summary>
        void m_watchdog_off_TimeOut()
        {
            Microsoft.SPOT.Hardware.Utility.Backlight(false);
        }

        //void m_distance_DistanceChanged(int value)
        //{
        //    if (!Dispatcher.CheckAccess())
        //    {
        //        Dispatcher.BeginInvoke(m_distanceUpdatedDelegate, value);
        //        return;
        //    }
        //    if (value < SAFE_DISTANCE)
        //    {
        //        Stop();
        //    }

        //    txt_freeahead_value.TextContent = value.ToString();
        //}

        Controls.Slider slider_speed;
        Controls.Slider slider_direction;
        Text txt_speed_value;
        Text txt_direction_value;
        Text txt_pwmleft_value;
        Text txt_pwmright_value;
        Text txt_freeahead_value;

        /// <summary>
        /// Adds controls to user interface - sliders, button, text...
        /// </summary>
        /// <param name="window"></param>
        void AddControls(ref Window window)
        {
            Font font = Resources.GetFont(Resources.FontResources.small);

            Canvas c = new Canvas();
            c.Width = window.Width;
            c.Height = window.Height;

            slider_speed = new Controls.Slider();
            slider_speed.MinValue = -7;
            slider_speed.MaxValue = 7;
            slider_speed.Height = 220;
            slider_speed.Width = 70;
            slider_speed.SliderSize = 20;
            slider_speed.SetMargin(10);
            slider_speed.Background = new Microsoft.SPOT.Presentation.Media.LinearGradientBrush(Microsoft.SPOT.Presentation.Media.Color.Black, Microsoft.SPOT.Presentation.Media.Color.White, 0, 0, slider_speed.ActualWidth, slider_speed.ActualHeight);
            slider_speed.ValueChanged += new Controls.Slider.ValueChangedHandler(slider_speed_ValueChanged);
            c.Children.Add(slider_speed);

            slider_direction = new Controls.Slider(Controls.Direction.HORIZONTAL);
            slider_direction.MinValue = -7;
            slider_direction.MaxValue = 7;
            slider_direction.Height = 70;
            slider_direction.Width = 220;
            slider_direction.SliderSize = 20;
            slider_direction.SetMargin(90, 160, 0, 0);
            slider_direction.Background = new Microsoft.SPOT.Presentation.Media.SolidColorBrush(Microsoft.SPOT.Presentation.Media.Colors.Gray);
            slider_direction.ValueChanged += new Controls.Slider.ValueChangedHandler(slider_direction_ValueChanged);
            c.Children.Add(slider_direction);

            Text txt_stop = new Text(font, "STOP");
            txt_stop.HorizontalAlignment = HorizontalAlignment.Center;
            txt_stop.VerticalAlignment = VerticalAlignment.Center;

            Border btn_stop = new Border();
            btn_stop.Width = 105;
            btn_stop.Height = 65;
            btn_stop.SetMargin(90, 85, 0, 0);
            btn_stop.Background = new Microsoft.SPOT.Presentation.Media.SolidColorBrush(Microsoft.SPOT.Presentation.Media.Colors.Red);
            btn_stop.BorderBrush = new Microsoft.SPOT.Presentation.Media.SolidColorBrush(Microsoft.SPOT.Presentation.Media.Color.Black);
            btn_stop.Child = txt_stop;
            btn_stop.StylusDown += new StylusEventHandler(btn_stop_StylusDown);
            c.Children.Add(btn_stop);

            // Status boxes
            Text txt_speed = new Text(font, "Speed:");
            txt_speed.SetMargin(90, 10, 0, 0);
            c.Children.Add(txt_speed);

            txt_speed_value = new Text(font, ((int)0).ToString());
            txt_speed_value.Width = 50;
            txt_speed_value.SetMargin(145, 10, 0, 0);
            txt_speed_value.TextAlignment = Microsoft.SPOT.Presentation.Media.TextAlignment.Right;
            c.Children.Add(txt_speed_value);

            Text txt_direction = new Text(font, "Direction:");
            txt_direction.SetMargin(90, 30, 0, 0);
            c.Children.Add(txt_direction);

            txt_direction_value = new Text(font, ((int)0).ToString());
            txt_direction_value.Width = 50;
            txt_direction_value.SetMargin(145, 30, 0, 0);
            txt_direction_value.TextAlignment = Microsoft.SPOT.Presentation.Media.TextAlignment.Right;
            c.Children.Add(txt_direction_value);

            Text txt_pwmleft = new Text(font, "PWM Left:");
            txt_pwmleft.SetMargin(205, 10, 0, 0);
            c.Children.Add(txt_pwmleft);

            txt_pwmleft_value = new Text(font, "");
            txt_pwmleft_value.Width = 50;
            txt_pwmleft_value.SetMargin(260, 10, 0, 0);
            txt_pwmleft_value.TextAlignment = Microsoft.SPOT.Presentation.Media.TextAlignment.Right;
            c.Children.Add(txt_pwmleft_value);

            Text txt_pwmright = new Text(font, "PWM Right:");
            txt_pwmright.SetMargin(205, 30, 0, 0);
            c.Children.Add(txt_pwmright);

            txt_pwmright_value = new Text(font, "");
            txt_pwmright_value.Width = 50;
            txt_pwmright_value.SetMargin(260, 30, 0, 0);
            txt_pwmright_value.TextAlignment = Microsoft.SPOT.Presentation.Media.TextAlignment.Right;
            c.Children.Add(txt_pwmright_value);

            Text txt_freeahead = new Text(font, "Free ahead:");
            txt_freeahead.SetMargin(90, 50, 0, 0);
            c.Children.Add(txt_freeahead);

            txt_freeahead_value = new Text(font, "N/A");
            txt_freeahead_value.Width = 50;
            txt_freeahead_value.SetMargin(145, 50, 0, 0);
            txt_freeahead_value.TextAlignment = Microsoft.SPOT.Presentation.Media.TextAlignment.Right;
            c.Children.Add(txt_freeahead_value);

            window.Child = c;
        }

        /// <summary>
        /// Happens when value of direction slider has changed.
        /// Updates robot controller's direction and updates status text
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="newValue"></param>
        void slider_direction_ValueChanged(object sender, int newValue)
        {
            controller.Direction = (short)(100 * newValue / 15);
            txt_direction_value.TextContent = newValue.ToString() + " / " + controller.Direction.ToString();
            UpdatePWMText();
        }

        /// <summary>
        /// Happens when value of speed slider has changed.
        /// Updates robot controller's speed and updates status text
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="newValue"></param>
        void slider_speed_ValueChanged(object sender, int newValue)
        {
            controller.Speed = (short)(100 * newValue / 15);
            txt_speed_value.TextContent = newValue.ToString() + " / " + controller.Speed.ToString();
            UpdatePWMText();
        }

        /// <summary>
        /// Updates values on PWM status text
        /// </summary>
        void UpdatePWMText()
        {
            txt_pwmleft_value.TextContent = controller.PWM_Left.ToString();
            txt_pwmright_value.TextContent = controller.PWM_Right.ToString();
        }

        /// <summary>
        /// Happens when stylus is put down on STOP button.
        /// Calls Stop()
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void btn_stop_StylusDown(object sender, StylusEventArgs e)
        {
            Stop();
        }

        /// <summary>
        /// Sets both speed and direction to 0
        /// </summary>
        void Stop()
        {
            slider_speed.Value = 0;
            slider_direction.Value = 0;
        }

        /// <summary>
        /// Happens when new byte is received from serial port.
        /// Converts byte to usable values and sends them on to the sliders
        /// </summary>
        /// <param name="value"></param>
        void listener_ByteReceived(byte value)
        {
            if (!Dispatcher.CheckAccess())
            {
                Dispatcher.BeginInvoke(m_byteReceivedDelegate, value);
                m_watchdog_on.Feed(1000);
                m_watchdog_off.Start(5000, false);
                m_watchdog_on.Start(1000, false);
                return;
            }

            // Parse byte - MS nibble is direction, LS nibble is speed
            //Debug.Print("Speed: " + ((((value & 0x08) == 0) ? 1 : -1) * (value & 0x7)).ToString());
            //Debug.Print("Direction: " + (((((value & 0x80) == 0) ? 1 : -1) * (value & 0x70)) >> 4).ToString());
            slider_speed.Value = (((value & 0x08) == 0) ? 1 : -1) * (value & 0x7);
            slider_direction.Value = ((((value & 0x80) == 0) ? 1 : -1) * (value & 0x70)) >> 4;
            //controller.Speed = (short)((((value & 0x08) == 0) ? 100 : -100) * (value & 0x07) / 15);
            //controller.Direction = (short)((((value & 0x80) == 0) ? 100 : -100) * ((value >> 4) & 0x07) / 15);
        }

        #region Hardware Buttons for testing
        private void OnButtonUp(object sender, ButtonEventArgs e)
        {
            // Print the button code to the Visual Studio output window.
            Debug.Print(e.Button.ToString());

            int speed = 0 + (((m_speed & 0x08) == 0) ? (m_speed & 0x07) : -(m_speed & 0x07));
            int direc = ((m_direction & 0x08) == 0) ? (m_direction & 0x07) : -(m_direction & 0x07);
            switch (e.Button.ToString())
            {
                case "37":
                    if (--direc < -7)
                    {
                        direc = -7;
                    }
                    break;
                case "38":
                    if (++speed > 7)
                    {
                        speed = 7;
                    }
                    break;
                case "39":
                    if (++direc > 7)
                    {
                        direc = 7;
                    }
                    break;
                case "40":
                    if (--speed < -7)
                    {
                        speed = -7;
                    }
                    break;
                case "41":
                    direc = 0;
                    speed = 0;
                    break;
            }

//            pwm.Configure(62, m_duty);
            m_speed = (byte)((((speed < 0 ? (speed * -1) : speed) & 0x07) | ((speed < 0) ? 0x08 : 0x00)) & 0x0F);
            m_direction = (byte)((((direc < 0 ? (direc * -1) : direc) & 0x07) | ((direc < 0) ? 0x08 : 0x00)) & 0x0F);
            Debug.Print("Speed: " + speed.ToString() + " : " + m_speed.ToString());
            Debug.Print("Direction: " + direc.ToString() + " : " + m_direction.ToString());
            listener_ByteReceived((byte)((m_direction << 4) | (m_speed & 0x0F)));
        }
        #endregion
    }
}
